/// File: Robotics4.NET\CMarshalGenerator\Program.cs
/// 
/// ------------------------------------------------------------
/// Copyright (c) 2006
///   Antonio Cisternino (cisterni@di.unipi.it),
///   Daniele Mazzei     (mazzei@di.unipi.it),
///   Federico Cicchi    (cicchi@di.unipi.it)
/// 
/// The use and distribution terms for this software are 
/// contained in the file named license.txt, which can be found 
/// in the root of this distribution.
/// By using this software in any fashion, you are agreeing to 
/// be bound by the terms of this license.
///
/// You must not remove this notice, or any other, from this
/// software.
/// ------------------------------------------------------------

// Known issues:
//
// CRITICAL
//
// long long are not supported, please avoid them
// Enumeration *should* be implemented
// Types are not closed

//#define PICMICRO

using System;
using System.Collections.Generic;
using System.Text;
using System.Xml;
using System.Xml.Serialization;
using System.Reflection;
using System.IO;

namespace CMarshalGenerator
#if !PICMICRO
{
    delegate void FieldProcessor(Type t, FieldInfo f, StreamWriter cf);

    class Program
    {
        static Dictionary<Type, bool> TypesToGenerate;
        static void VisitFields(Type bt, FieldProcessor p, Type t, StreamWriter cf) 
        {
            if (bt != typeof(Robotics4.NET.RobletMessage))
                VisitFields(bt.BaseType, p, t, cf);

            foreach (FieldInfo f in bt.GetFields(BindingFlags.DeclaredOnly | BindingFlags.Instance | BindingFlags.Public))
                p(t, f, cf);
        }

        static void GenerateEnum(Type t, StreamWriter hf)
        {
            TypesToGenerate[t] = true;
            bool first = true;
            hf.WriteLine("typedef enum {");
            foreach (FieldInfo f in t.GetFields())
            {
                if (f.FieldType != t) continue;
                if (first) first = false;
                else hf.WriteLine(",");
                hf.Write("  {0}_{1} = {2}", t.Name, f.Name, (int)f.GetValue(null));
            }
            hf.WriteLine();
            hf.WriteLine("}} {0};", t.Name);

            hf.WriteLine();
            hf.WriteLine("int {0}_read(char* s);", t.Name);
            hf.WriteLine("int {0}_write(int v, char *dest, int len);", t.Name);
        }

        static void GenerateStruct(Type t, StreamWriter hf)
        {
            Dictionary<Type, string> map = new Dictionary<Type, string>();
            map[typeof(string)] = "char*";
            map[typeof(int)] = "int";
            map[typeof(bool)] = "int";
            map[typeof(char)] = "short";
            map[typeof(short)] = "short";
            map[typeof(float)] = "float";
            map[typeof(double)] = "double";
            map[typeof(byte)] = "char";
            map[typeof(long)] = "long"; // FIXME: Check this

            List<Type> included = new List<Type>();
            foreach (FieldInfo f in t.GetFields())
            {
                if (!map.ContainsKey(f.FieldType))
                {
                    if (!TypesToGenerate.ContainsKey(f.FieldType))
                        TypesToGenerate[f.FieldType] = false;

                    if (!included.Contains(f.FieldType))
                    {
                        hf.WriteLine("#include \"{0}.h\"", f.FieldType.Name);
                        included.Add(f.FieldType);
                    }
                }
            }

            TypesToGenerate[t] = true;
            hf.WriteLine();
            hf.WriteLine(string.Format("typedef struct _{0} {{", t.Name));

            FieldInfo[] flds = t.GetFields();
            List<FieldInfo> fldsnew = new List<FieldInfo>();
            int fdx = flds.Length - 1;
            while (fdx >= 0)
            {
                // search backward
                while (fdx > 0 && flds[fdx - 1].DeclaringType == flds[fdx].DeclaringType) fdx--;
                for (int i = fdx; i < flds.Length && flds[i].DeclaringType == flds[fdx].DeclaringType; i++)
                    fldsnew.Add(flds[i]);
                fdx--;
            }

            foreach (FieldInfo f in fldsnew)
            {
                //Console.WriteLine(f.FieldType);
                if (map.ContainsKey(f.FieldType))
                    hf.WriteLine("  {0} {1};", map[f.FieldType], f.Name);
                else
                    hf.WriteLine("  {0} {1};", f.FieldType.Name, f.Name);
            }
            hf.WriteLine(string.Format("}} {0};", t.Name));

            hf.WriteLine();
            hf.WriteLine("int {0}_startsWith(char* buf, int len);", t.Name);
            hf.WriteLine("int {0}_Read({0}* dest, char* buf, int len);", t.Name);
            hf.WriteLine("char* {0}_Write({0}* src, char* dest, int len);", t.Name);
        }

        static void GenerateStartsWith(Type t, StreamWriter cf)
        {
            cf.WriteLine();
            cf.WriteLine("int {0}_startsWith(char* buf, int len)", t.Name);
            cf.WriteLine("{");
            cf.WriteLine(
@"  char* start = buf;
  int nc = len;
  SKIPBLANKS();
  MATCH(""<?"");
  SKIPBLANKS();
  MATCH(""xml"");
  GOTO(""?>"");
  SKIPBLANKS();
  MATCH(""<{0}"");
  return (*buf == ' ' || *buf == '\t' || *buf == '\n' || *buf == '\r' || *buf == '>');", t.Name);
            
            cf.WriteLine("}");
        }

        static void GenerateReadField(Type t, FieldInfo f, StreamWriter cf)
        {
            Dictionary<Type, string> map = new Dictionary<Type,string>();
            map[typeof(string)] = "";
            map[typeof(int)] = "atoi";
            map[typeof(bool)] = "PARSEBOOL";
            map[typeof(char)] = "";
            map[typeof(short)] = "(short)atoi";
            map[typeof(float)] = "(float)atof";
            map[typeof(double)] = "atof";
            map[typeof(byte)] = "(char)atoi";
            map[typeof(long)] = "atol"; // FIXME: Check this

            if (map.ContainsKey(f.FieldType))
                cf.WriteLine(
@"  SKIPBLANKS();
  MATCH(""<{0}>"");
  if (!(p = find(buf, ""</{0}>"", nc))) return 0;
  *(p - {1}) = 0;
  dest->{0} = {2}(buf);
  buf = p;
  if ((nc = len - (int)(buf - start)) < 0) return 0;
", f.Name, f.Name.Length + 3, map[f.FieldType]);
            else if (f.FieldType.IsEnum)
                cf.WriteLine(
@"  SKIPBLANKS();
  MATCH(""<{0}>"");
  if (!(p = find(buf, ""</{0}>"", nc))) return 0;
  *(p - {1}) = 0;
  dest->{0} = {2}_read(buf);
  buf = p;
  if ((nc = len - (int)(buf - start)) < 0) return 0;
", f.Name, f.Name.Length + 3, f.FieldType.Name);
            else // FIXME: do not handle nested structs of the same type!
                cf.WriteLine(
@"  SKIPBLANKS();
  MATCH(""<{0}>"");
  if (!(p = find(buf, ""</{0}>"", nc))) return 0;
  *(p - {1}) = 0;
  {2}_Read(&dest->{0}, buf, p - buf);
  buf = p;
  if ((nc = len - (int)(buf - start)) < 0) return 0;
", f.Name, f.Name.Length + 3, map[f.FieldType]);
        }

        static void GenerateRead(Type t, StreamWriter cf)
        {
            cf.WriteLine();
            cf.WriteLine("int {0}_Read({0}* dest, char* buf, int len)", t.Name);
            cf.WriteLine("{");
            cf.WriteLine(
@"  char *start = buf;
  char *p;
  int nc = len;
  
  SKIPBLANKS();
  MATCH(""<?"")
  SKIPBLANKS();
  MATCH(""xml"");
  GOTO(""?>"");
  SKIPBLANKS();
  MATCH(""<{0}"");
  GOTO("">"");
",  t.Name);

            VisitFields(t, new FieldProcessor(GenerateReadField), t, cf);

            cf.WriteLine("  return 1;");
            cf.WriteLine("}");
        }

        static void GenerateWriteField(Type t, FieldInfo f, StreamWriter cf)
        {
            string openmsg = string.Format("\"  <{0}>\"", f.Name);
            string closemsg = string.Format("\"</{0}>\\n\"", f.Name);
            cf.WriteLine("  CHK(sprintf(dest, {0}));", openmsg);

            Dictionary<Type, string> map = new Dictionary<Type,string>();
            map[typeof(string)] = "%s";
            map[typeof(int)] = "%d";
            map[typeof(char)] = "%c";
            map[typeof(short)] = "%d";
            map[typeof(float)] = "%f";
            map[typeof(double)] = "%f";
            map[typeof(byte)] = "%c";
            map[typeof(long)] = "%d"; // FIXME: Check this
            if (map.ContainsKey(f.FieldType))
                cf.WriteLine("  CHK(sprintf(dest, \"{0}\", src->{1}));", map[f.FieldType], f.Name);
            else if (f.FieldType.IsEnum)
                cf.WriteLine("  CHK({0}_write(src->{1}, dest, len));", f.FieldType.Name, f.Name);
            else if (f.FieldType == typeof(bool))
                cf.WriteLine("  CHK(sprintf(dest, \"%s\", src->{0} ? \"true\" : \"false\"));", f.Name);
            else
                cf.WriteLine("  CHK({0}_write(&src->{1}, dest, len));", f.FieldType.Name, f.Name);

            cf.WriteLine("  CHK(sprintf(dest, {0}));", closemsg);
        }

        static void GenerateWrite(Type t, StreamWriter cf)
        {
            string xmlhead = "\"<?xml version=\\\"1.0\\\" encoding=\\\"utf-8\\\" ?>\\n\"";
            string openmsg = string.Format("\"<{0} xmlns:xsi=\\\"http://www.w3.org/2001/XMLSchema-instance\\\" xmlns:xsd=\\\"http://www.w3.org/2001/XMLSchema\\\">\\n\"", t.Name);
            string closemsg = string.Format("\"</{0}>\\n\"", t.Name);

            cf.WriteLine();
            cf.WriteLine("char* {0}_Write({0}* src, char* dest, int len)", t.Name);
            cf.WriteLine("{");
            cf.WriteLine("  CHK(sprintf(dest, {0}));", xmlhead);

            cf.WriteLine("  CHK(sprintf(dest, {0}));", openmsg);

            VisitFields(t, new FieldProcessor(GenerateWriteField), t, cf);

            cf.WriteLine("  CHK(sprintf(dest, {0}));", closemsg);

            cf.WriteLine("  return dest;");
            cf.WriteLine("}");
        }

        static void GenerateEnumRead(Type t, StreamWriter cf)
        {
            cf.WriteLine();
            cf.WriteLine("int {0}_read(char* s) {{", t.Name);
            foreach (FieldInfo f in t.GetFields())
            {
                if (f.FieldType != t) continue;
                if ((int)f.GetValue(null) == -1)
                    Console.WriteLine("Dhe, ma popo -1 as enum dovevi sceglie... vedi che {0}_read va rivista a mano o mi patchi...", t.Name);
                cf.WriteLine("  if (!strcmp(s, \"{0}\")) return {1};", f.Name, (int)f.GetValue(null));
            }
            cf.WriteLine("  return -1;");
            cf.WriteLine("}");
        }

        static void GenerateEnumWrite(Type t, StreamWriter cf)
        {
            cf.WriteLine();
            cf.WriteLine("int {0}_write(int v, char* dest, int len) {{", t.Name);
            cf.WriteLine("  switch (v) {");
            foreach (FieldInfo f in t.GetFields())
            {
                if (f.FieldType != t) continue;
                cf.WriteLine("    case {0}: CHK(sprintf(dest, \"{1}\")); return {2};", (int)f.GetValue(null), f.Name, f.Name.Length);
            }
            cf.WriteLine("  }");
            cf.WriteLine("  return 0;");
            cf.WriteLine("}");
        }

        static void GenerateFunctions(Type t, StreamWriter cf)
        {
            if (t.IsEnum)
            {
                GenerateEnumRead(t, cf);
                GenerateEnumWrite(t, cf);
            }
            else
            {
                GenerateStartsWith(t, cf);
                GenerateRead(t, cf);
                GenerateWrite(t, cf);
            }
        }

        static void GenerateTypeInterface(Type t)
        {
            Console.WriteLine("Generating interface for type {0}", t);
            StreamWriter hf = File.CreateText(string.Format("Messages\\{0}.h", t.Name));

            hf.WriteLine("#ifndef _{0}_H_", t.Name.ToUpper());
            hf.WriteLine("#define _{0}_H_", t.Name.ToUpper());
            hf.WriteLine();

            if (t.IsEnum)
                GenerateEnum(t, hf);
            else
                GenerateStruct(t, hf);

            hf.WriteLine();
            hf.WriteLine(string.Format("#endif // _{0}_H_", t.Name.ToUpper()));
            hf.WriteLine();

            hf.Close();

            StreamWriter cf = File.CreateText(string.Format("Messages\\{0}.c", t.Name));

            cf.WriteLine("#include \"{0}.h\"", t.Name);
            cf.WriteLine("#include <string.h>");
            cf.WriteLine("#include <stdlib.h>");
            cf.WriteLine("#include <stdio.h>");
            cf.WriteLine();
            cf.WriteLine("#define CHK(l) { int i = (l); dest += i; len -= i; if (len < 0) return 0; }");
            cf.WriteLine("#define MATCH(TOK)   if (!(buf = match(buf, TOK))) return 0;\\");
            cf.WriteLine("                     if ((nc = len - (int)(buf - start)) < 0) return 0;");
            cf.WriteLine("#define GOTO(TOK)	 buf = find(buf, TOK, nc); \\");
            cf.WriteLine("                   if ((nc = len - (int)(buf - start)) < 0) return 0;");
            cf.WriteLine("#define SKIPBLANKS()   buf = skipBlanks(buf, nc); \\");
            cf.WriteLine("                       if ((nc = len - (int)(buf - start)) < 0) return 0;");
            cf.WriteLine("#define PARSEBOOL(buf) (strcmp(\"true\", buf) == 0)");
            cf.WriteLine();
            cf.WriteLine(
@"static char* skipBlanks(char* s, int len) {
  while (len-- && *s && (*s == ' ' || *s == '\t' || *s == '\n' || *s == '\r')) s++;
  return s;
}

static char* match(char* b, char* t) {
  if (!strncmp(b, t, strlen(t)))
    return b + strlen(t);
  return 0;
}

static char* find(char* b, char* t, int len) {
  while (len-- && *b && !match(b, t)) b++;
  if (len && *b)
    return b + strlen(t);
  return 0;
}
");
            GenerateFunctions(t, cf);

            cf.Close();
        }

        static void GenerateNetOS(Dictionary<Type, bool> TypesToGenerate)
        {
            StreamWriter f = File.CreateText("uTNetOS\\uTNetOS.h");
            f.WriteLine("#ifndef _UTNETOS_H_");
            f.WriteLine("#define _UTNETOS_H_");
            f.WriteLine();
            f.WriteLine("#ifndef EMULATED_BUILD");
            f.WriteLine("#include <wf5282.h>         // The wildfire RTL all RTL functionality");
            f.WriteLine("#include <opentcp.h>");
            f.WriteLine("#define UDP_OPT_NONE 0");
            f.WriteLine("#else");
            f.WriteLine("#include \"WildFireEmul.h\"");
            f.WriteLine("#endif");
            f.WriteLine();
            f.WriteLine("#include \"heap.h\"");
            f.WriteLine("#include \"shared_timer.h\"");
            f.WriteLine("#include \"listeners.h\"");
            f.WriteLine("#include \"Config.h\"");
            f.WriteLine();
            f.WriteLine("#include \"uTNetOSMessages.h\"");
            f.WriteLine();
            f.WriteLine("#define MSGBUF (char*)(OTCP_TXBUF+UDP_APP_OFFSET)");
            f.WriteLine("#define MSGBUFSZ NETWORK_TX_BUFFER_SIZE-UDP_APP_OFFSET");
            f.WriteLine(@"
typedef int (*STARTS_WITH)(char* buf, int len);
typedef int (*READMSG)(void* dest, char* buf, int len);
typedef char* (*WRITEMSG)(void* src, char* dest, int len);

// FIXME: Perhaps it is better to have it generated from RobletMessage
typedef struct _uTNetOS_RobletMessage {
  char* Name;
  int RobletID;
  char* BrokerHost;
  int BrokerPort;
  PacketType Type;
} uTNetOS_RobletMessage;

void uTNetOS_Init(char* robletname, unsigned short port, int bodymapip, short bodymapport);
void uTNetOS_DoCycle();
void uTNetOS_ListenForEvent(FSABase* w, uTNetOS_RobletMessages msg);
void uTNetOS_SendMessage(WRITEMSG fw, void* src);
int uTNetOS_BodyMapIP();
short uTNetOS_BodyMapPort();
int uTNetOS_BrokerIP();
short uTNetOS_BrokerPort();
char* uTNetOS_RobletName();

extern Timer timer;");
            f.WriteLine("#define SECS(s) (s * 1000000)");
            f.WriteLine("#define MILLIS(s) (s * 1000)");
            f.WriteLine();
            f.WriteLine("#if defined(EMULATED_BUILD) && defined(WIN32)");
            f.WriteLine();
            f.WriteLine("#include <windows.h>");
            f.WriteLine();
            f.WriteLine("#define GETTIME() (GetTickCount() * 1000)");
            f.WriteLine("#define DELAY(t) Sleep(t/1000)");
            f.WriteLine(@"
#elif !defined(EMULATED_BUILD)

HEAPTIME uTNetOS_GetTime();

// GETTIME() macro should return a type compatible with HEAPTIME
// Estimated on 10 secs of running time and reported in microseconds.
// Error is around 2/10th of millisecond in 10 secs
//#define GETTIME() ((int)(_clock_count * 75.1612))
#define GETTIME() uTNetOS_GetTime()
#define DELAY(t) delay(t)
#endif");
            f.WriteLine();
            f.WriteLine("#endif // _UTNETOS_H_");
            f.Close();

            f = File.CreateText("uTNetOS\\uTNetOS.c");
            f.WriteLine("#include \"uTNetOS.h\"");
            f.WriteLine();
            f.WriteLine("#include <stdlib.h>");
            f.WriteLine("#include <malloc.h>");
            f.WriteLine("#include <stdio.h>");
            f.WriteLine("#include <string.h>");
            f.WriteLine("unsigned int _clock_count;");
            f.WriteLine("unsigned int _last_clock_count;");
            f.WriteLine("Timer timer;");
            f.WriteLine("// Socket to use to send and receive");
            f.WriteLine("char udp_socket;");
            f.WriteLine();
            f.WriteLine("#include \"uTNetOSMessagesInternal.h\"");
            f.WriteLine(@"
char IncomingBuf[MSGBUFSZ];
char ReadBuf[MSGBUFSZ];

int _broker_ip;
char _broker_ip_s[32];
short _broker_port;
int _bodymap_ip;
short _bodymap_port;
char *_roblet_name;

int uTNetOS_BodyMapIP() {
	return _bodymap_ip;
}

short uTNetOS_BodyMapPort() {
	return _bodymap_port;
}

int uTNetOS_BrokerIP() {
	return _broker_ip;
}

short uTNetOS_BrokerPort() {
	return _broker_port;
}

char* uTNetOS_RobletName() {
	return _roblet_name;
}

void uTNetOS_ListenForEvent(FSABase* w, uTNetOS_RobletMessages msg) {
  listeners_Insert(&listeners[msg], w);
}

/*
int uTNetOS_pit_handler( void *arg1, void *arg2 ) { 
	_clock_count++;
	if (!_clock_count)
		TimerSetZeroTime(&timer);
	pit_flag_clear( PIT_CHANNEL0 );
	return( 1 );
}
*/

void _SendMessage(int sz) {
	//char sock = udp_getsocket(0, 0, UDP_OPT_NONE);
//	printf(""%d.%d.%d.%d\n"", IP1(_bodymap_ip), IP2(_bodymap_ip), IP3(_bodymap_ip), IP4(_bodymap_ip));
//	printf(""%d\n"", _bodymap_port);
//	printf(""Send to %d.%d.%d.%d %d sz %d\n\n%s\n"", IP1(_bodymap_ip), IP2(_bodymap_ip), IP3(_bodymap_ip), IP4(_bodymap_ip), _bodymap_port, sz, MSGBUF);
	
	udp_send(udp_socket, _bodymap_ip, _bodymap_port, MSGBUF, MSGBUFSZ, sz);
	//udp_close(sock);
}

int udp_eventlistener( char handle, char evt, int ip, short port, short buffindex, short datalen ) {
	int i;
	printf(""Packet from IP Address: %d.%d.%d.%d\n"", IP1(ip), IP2(ip), IP3(ip), IP4(ip) );
	switch(evt) {
		case UDP_EVENT_DATA:
			RECEIVE_NETWORK_BUF(IncomingBuf, datalen);
            if (MetaDataRequest_startsWith(IncomingBuf, datalen)) {
				char h[32], *p;
				MetaDataRequest req;
				MetaDataRequest_Read(&req, IncomingBuf, datalen);
				sprintf(h, ""%d.%d.%d.%d"", IP1(ip), IP2(ip), IP3(ip), IP4(ip));
				p = RobletInterface_Write(_roblet_name, req.BrokerHost, req.BrokerPort, MSGBUF, MSGBUFSZ);
				_SendMessage(p - MSGBUF);
            } else {
			    for (i = 0; i < known_messages_length; i++) {
				    if (known_messages[i](IncomingBuf, datalen)) {
					    known_messages_read[i](ReadBuf, IncomingBuf, datalen);
				   	    listeners_Notify(&listeners[i], ReadBuf, i);
			    	}
			    }
            }
			break;
		default:
			puts( ""Unknown UDP event"" );
	} 
	return( 0 ); 
}

#ifndef EMULATED_BUILD
HEAPTIME uTNetOS_GetTime() {
	int tick = (1<<15) - pit_get(PIT_CHANNEL1);
	if (tick < _last_clock_count) {
		_clock_count++;
		//printf(""count is %d and tick %d\n"", _clock_count, tick);
		
	}
	_last_clock_count = tick;
	return (((((HEAPTIME)_clock_count) << 15) | ((HEAPTIME)tick)) * ((HEAPTIME)6405))/100; // 1 tick is 640 us with an error of 0.1us
}
#endif

void uTNetOS_Init(char* robletname, unsigned short port, int bodymapip, short bodymapport) {
	int i;
//	printf(""Before set\n"");
	_broker_port = port;
	_bodymap_ip = bodymapip;
	_bodymap_port = bodymapport;
	_roblet_name = strdup(robletname);

//	printf(""Before heap_init\n"");
	// FIXME: perhaps in future version the initial size should be exposed
	heap_Init(&(timer.priorityQueue), 16);

//	printf(""Before listeners\n"");
	for (i = 0; i < 1; i++)
	  listeners_Init(&listeners[i], 1);

//	printf(""Before tcp_init\n"");
	tcpip_init(0); // FIXME: how to deal with DHCP?

	_broker_ip = localmachine.localip;
	sprintf(_broker_ip_s, ""%d.%d.%d.%d"", IP1(_broker_ip), IP2(_broker_ip), IP3(_broker_ip), IP4(_broker_ip));
	printf(""%d.%d.%d.%d\n"", IP1(_broker_ip), IP2(_broker_ip), IP3(_broker_ip), IP4(_broker_ip));

	udp_socket = udp_getsocket(0 , udp_eventlistener, UDP_OPT_SEND_CS | UDP_OPT_CHECK_CS);
	printf(""sock handler %d\n"", udp_socket);
	if( udp_socket == -1 ) {
		printf(""DNS: No free UDP sockets!! \r\n"");
		return;
	}
	printf(""port %d\n"", port);
	
	if( udp_open( udp_socket, port ) >= 0 ) {
		printf(""Opened UDP socket on local port %d\n"", port );
	}
	
	_clock_count = 0;
	_last_clock_count = 0; 
	// Initialize the programmable interrupt timer
	pit_init( PIT_CHANNEL1, // Channel 0
		PIT_CLK_DIV4096, // 64MHz/64 = 1 MHz 
		1<<15, // 1<<15
		CF_ISR_LEVEL4 | CF_ISR_PRIOR0, NULL);
		//CF_ISR_LEVEL4 | CF_ISR_PRIOR0, 
		//uTNetOS_pit_handler ); // address of the handler
}

void uTNetOS_DoCycle() {
	tcp_tick();
#if defined(EMULATED_BUILD)
	DELAY(TimerDoStep(&timer));
#else
	TimerDoStep(&timer);
#endif
}

void uTNetOS_SendMessage(WRITEMSG fw, void* src) {
	uTNetOS_RobletMessage *msg = (uTNetOS_RobletMessage*)src;
	char* p;
	int sz;

	msg->BrokerHost = _broker_ip_s;
	msg->BrokerPort = _broker_port;
	msg->Name = _roblet_name;
	msg->RobletID = 0;
	msg->Type = PacketType_Data;

	p = fw(src, MSGBUF, MSGBUFSZ);
	sz = p - MSGBUF;

	_SendMessage(sz);
}
");
            f.Close();
            bool first = true;
            StringBuilder sb = new StringBuilder();
            StringBuilder mn = new StringBuilder();
            f = File.CreateText("uTNetOS\\uTNetOSMessages.h");
            foreach (Type t in TypesToGenerate.Keys)
            {
                if (!t.IsSubclassOf(typeof(Robotics4.NET.RobletMessage))) continue;
                if (first) first = false;
                else mn.Append(", ");
                
                sb.AppendLine(string.Format("#include \"{0}.h\"", t.Name));
                mn.AppendFormat("RobletMessages_{0}", t.Name);
            }
            f.WriteLine("#ifndef _UTNETOSMESSAGES_H__");
            f.WriteLine("#define _UTNETOSMESSAGES_H__");
            f.WriteLine();
            f.WriteLine("#include \"uTNetOSRobletInterface.h\"");
            f.WriteLine("{0}",sb.ToString());
            f.WriteLine();
            f.WriteLine("typedef enum {{ {0} }} uTNetOS_RobletMessages;",mn.ToString());
            f.WriteLine();
            f.WriteLine("#endif // _UTNETOSMESSAGES_H__");
            f.Close();

            StringBuilder km = new StringBuilder();
            StringBuilder kmr = new StringBuilder();
            StringBuilder kmw = new StringBuilder();
            first = true;
            int count = 0;
            foreach (Type t in TypesToGenerate.Keys)
            {
                if (!t.IsSubclassOf(typeof(Robotics4.NET.RobletMessage))) continue;
                count++;
                if (first) first = false;
                else
                {
                    km.Append(", ");
                    kmr.Append(", ");
                    kmw.Append(", ");
                }
                km.Append(string.Format("{0}_startsWith", t.Name));
                kmr.Append(string.Format("(READMSG){0}_Read", t.Name));
                kmw.Append(string.Format("(WRITEMSG){0}_Write", t.Name));
            }
            f = File.CreateText("uTNetOS\\uTNetOSMessagesInternal.h");
            f.WriteLine("#ifndef _UTNETOSMESSAGESINTERNAL_H_");
            f.WriteLine("#define _UTNETOSMESSAGESINTERNAL_H_");
            f.WriteLine(@"
STARTS_WITH known_messages[] = {{ {0} }};
READMSG known_messages_read[] = {{ {1} }};
WRITEMSG known_messages_write[] = {{ {2} }};
int known_messages_length = {3};
Listeners listeners[{3}]; // Tipo dei messaggi", km.ToString(), kmr.ToString(), kmw.ToString(), count);
            f.WriteLine();
            f.WriteLine("#endif // _UTNETOSMESSAGESINTERNAL_H_");
            f.Close();
        }

        static void Main(string[] args)
        {
            if (!Directory.Exists("Messages"))
                Directory.CreateDirectory("Messages");
            if (!Directory.Exists("uTNetOS"))
                Directory.CreateDirectory("uTNetOS");
            TypesToGenerate = new Dictionary<Type, bool>();
            foreach (string s in args)
            {
                if (!File.Exists(s))
                {
                    Console.WriteLine("Could not find {0}, skipping...", s);
                    continue;
                }
                string fn = (new FileInfo(s)).FullName;
                Assembly a = Assembly.LoadFile(fn);
                foreach (Type t in a.GetTypes())
                {
                    if (t.IsSubclassOf(typeof(Robotics4.NET.RobletMessage)))
                    {
                        TypesToGenerate[t] = false;
                        //GenerateTypeInterface(t);
                        //RobletTest.Beat rt = new RobletTest.Beat();
                        //XmlSerializer ser = new XmlSerializer(t);
                        //ser.Serialize(Console.Out, rt);
                    }
                    else if (t.IsSubclassOf(typeof(Robotics4.NET.RobletBase)))
                    {
                        Robotics4.NET.RobletBase rob = (Robotics4.NET.RobletBase)Activator.CreateInstance(t);
                        Robotics4.NET.RobletInterface interf;
                        Type[] types;
                        Robotics4.NET.RobletBroker.Init();
                        rob.GetRobletInterface(out interf, out types);
                        interf.RobletID = 0;
                        interf.BrokerPort = 0;
                        interf.BrokerHost = "placeholder";
                        interf.Name = "placeholder";
                        XmlSerializer ser = new XmlSerializer(interf.GetType(), null, types, null, null, (new FileInfo(fn)).Directory.FullName, null);
                        MemoryStream mem = new MemoryStream();
                        ser.Serialize(mem, interf);
                        string data = System.Text.UTF8Encoding.UTF8.GetString(mem.GetBuffer());
                        data = System.Text.RegularExpressions.Regex.Replace(data, "<BrokerHost>[^<]*</BrokerHost>", "<BrokerHost><<HOST>></BrokerHost>");
                        data = System.Text.RegularExpressions.Regex.Replace(data, "<BrokerPort>[^<]*</BrokerPort>", "<BrokerPort><<PORT>></BrokerPort>");
                        data = System.Text.RegularExpressions.Regex.Replace(data, "<RobletID>[^<]*</RobletID>", "<RobletID><<ID>></RobletID>");
                        data = System.Text.RegularExpressions.Regex.Replace(data, "<Name>[^<]*</Name>", "<Name><<NAME>></Name>");
                        data = System.Text.RegularExpressions.Regex.Replace(data, "</RobletInterface>.*", "</RobletInterface>");
                        //data = data.Replace("\\", "\\\\");
                        //data = data.Replace("\r", "");
                        //data = data.Replace("\n", "\\r\\n");
                        //data = data.Replace("\"", "\\\"");
                        //string[] frags = System.Text.RegularExpressions.Regex.Split(data, "(<(?:BrokerHost|BrokerPort|RobletID|Name)>[^<]*</(?:BrokerHost|BrokerPort|RobletID|Name)>)");

                        StreamWriter f = File.CreateText("uTNetOS\\uTNetOSRobletInterface.h");
                        f.WriteLine("#ifndef _UTNETOSROBLETINTERFACE_H__");
                        f.WriteLine("#define _UTNETOSROBLETINTERFACE_H__");
                        f.WriteLine();
                        f.WriteLine("char* RobletInterface_Write(char* robletname, char* host, int port, char* dest, int len);");
                        f.WriteLine();
                        f.WriteLine("#endif // _UTNETOSROBLETINTERFACE_H__");
                        f.Close();

                        f = File.CreateText("RobletInterface.xml");
                        f.WriteLine(data);
                        f.Close();

                        f = File.CreateText("uTNetOS\\uTNetOSRobletInterface.c");
                        f.WriteLine("#include \"uTNetOSRobletInterface.h\"");
                        f.WriteLine("#include <string.h>");
                        f.WriteLine("#include <stdlib.h>");
                        f.WriteLine("#include <stdio.h>");
                        f.WriteLine();
                        f.WriteLine("#define CHK(l) { int i = (l); dest += i; len -= i; if (len < 0) return 0; }");
                        f.WriteLine();
                        f.WriteLine("char* RobletInterface_Write(char* robletname, char* host, int port, char* dest, int len) {");
                        f.WriteLine("  CHK(sprintf(dest, \"<?xml version=\\\"1.0\\\"?>\\r\\n<RobletInterface InterfaceOnBodyMap=\\\"%s.xml\\\" xmlns:xsi=\\\"http://www.w3.org/2001/XMLSchema-instance\\\" xmlns:xsd=\\\"http://www.w3.org/2001/XMLSchema\\\">\\r\\n  <Name>%s</Name>\\r\\n  <RobletID>0</RobletID>\\r\\n  \", robletname, robletname));");
                        f.WriteLine("  CHK(sprintf(dest, \"<BrokerHost>%s</BrokerHost>\", host));");
                        f.WriteLine("  CHK(sprintf(dest, \"\\r\\n  \"));");
                        f.WriteLine("  CHK(sprintf(dest, \"<BrokerPort>%d</BrokerPort>\", port));");
                        f.WriteLine("  CHK(sprintf(dest, \"\\r\\n  <Type>Data</Type>\\r\\n  \\r\\n</RobletInterface>\"));");
                        f.WriteLine("  return dest;");
                        f.WriteLine("}");
                        f.Close();
                    }
                }
            }
            TypesToGenerate[typeof(Robotics4.NET.MetaDataRequest)] = false;

            bool exit = false;
            while (!exit)
            {
                exit = true;
                foreach (Type t in TypesToGenerate.Keys)
                {
                    if (!TypesToGenerate[t])
                    {
                        GenerateTypeInterface(t);
                        TypesToGenerate[t] = true;
                        exit = false;
                        break;
                    }
                }
            }
            GenerateNetOS(TypesToGenerate);
        }
    }
}
#else //Begin PICMICRO section
{
    delegate void FieldProcessor(Type t, FieldInfo f, StreamWriter cf);

    class Program
    {
        static XmlBinManager.MapGenerator xbmg;
        
        static Dictionary<Type, bool> TypesToGenerate;
        static void VisitFields(Type bt, FieldProcessor p, Type t, StreamWriter cf) 
        {
            if (bt != typeof(Robotics4.NET.RobletMessage))
                VisitFields(bt.BaseType, p, t, cf);

            foreach (FieldInfo f in bt.GetFields(BindingFlags.DeclaredOnly | BindingFlags.Instance | BindingFlags.Public))
            {
                p(t, f, cf);
            }
        }

        static void GenerateEnum(Type t, StreamWriter hf)
        {
            TypesToGenerate[t] = true;
            bool first = true;
            hf.WriteLine("typedef enum {");
            foreach (FieldInfo f in t.GetFields())
            {
                if (f.FieldType != t) continue;
                if (first) first = false;
                else hf.WriteLine(",");
                hf.Write("  {0}_{1} = {2}", t.Name, f.Name, (int)f.GetValue(null));
            }
            hf.WriteLine();
            hf.WriteLine("}} {0};", t.Name);

            hf.WriteLine();
            hf.WriteLine("int {0}_read(char* s);", t.Name);
            hf.WriteLine("int {0}_write(int v, char *dest, int len);", t.Name);
        }

        static void GenerateStruct(Type t, StreamWriter hf)
        {
            Dictionary<Type, string> map = new Dictionary<Type, string>();
            map[typeof(string)] = "char*";
            map[typeof(int)] = "int";
            map[typeof(bool)] = "int";
            map[typeof(char)] = "short";
            map[typeof(short)] = "short";
            map[typeof(float)] = "float";
            map[typeof(double)] = "double";
            map[typeof(byte)] = "char";
            map[typeof(long)] = "long"; // FIXME: Check this

            List<Type> included = new List<Type>();
            foreach (FieldInfo f in t.GetFields())
            {
                if (!map.ContainsKey(f.FieldType))
                {
                    if (!TypesToGenerate.ContainsKey(f.FieldType))
                        TypesToGenerate[f.FieldType] = false;

                    if (!included.Contains(f.FieldType))
                    {
                        hf.WriteLine("#include \"..\\Messages\\{0}.h\"", f.FieldType.Name);
                        included.Add(f.FieldType);
                    }
                }
            }

            TypesToGenerate[t] = true;
            hf.WriteLine();
            hf.WriteLine(string.Format("typedef struct _{0} {{", t.Name));

            FieldInfo[] flds = t.GetFields();
            List<FieldInfo> fldsnew = new List<FieldInfo>();
            int fdx = flds.Length - 1;
            while (fdx >= 0)
            {
                // search backward
                while (fdx > 0 && flds[fdx - 1].DeclaringType == flds[fdx].DeclaringType) fdx--;
                for (int i = fdx; i < flds.Length && flds[i].DeclaringType == flds[fdx].DeclaringType; i++)
                    fldsnew.Add(flds[i]);
                fdx--;
            }

            foreach (FieldInfo f in fldsnew)
            {
                //Console.WriteLine(f.FieldType);
                if (map.ContainsKey(f.FieldType))
                    hf.WriteLine("  {0} {1};", map[f.FieldType], f.Name);
                else
                    hf.WriteLine("  {0} {1};", f.FieldType.Name, f.Name);
            }
            hf.WriteLine(string.Format("}} {0};", t.Name));

            hf.WriteLine();
            hf.WriteLine("int {0}_startsWith(char* buf, int len);", t.Name);
            hf.WriteLine("int {0}_Read({0}* dest, char* buf, int len);", t.Name);
            hf.WriteLine("char* {0}_Write({0}* src, char* dest, int len);", t.Name);
        }

        static void GenerateStartsWith(Type t, String bc, StreamWriter cf)
        {
            cf.WriteLine();
            cf.WriteLine("int {0}_startsWith(char* buf, int len)", t.Name);
            cf.WriteLine("{");
            cf.WriteLine(
@"  char* start = buf;
  int nc = len;
  SKIPBLANKS();
  MATCH({0});
  SKIPBLANKS();
  MATCH({1});
  return 1;", xbmg.OpenXmlTagCode, bc);
            cf.WriteLine("}");
        }

        static void GenerateReadField(Type t, FieldInfo f, StreamWriter cf)
        {
            string openmsg = string.Format("{0}", xbmg.GenerateNextBinaryCode("<" + f.Name + ">"));
            string closemsg = string.Format("{0}", xbmg.GenerateNextBinaryCode("</" + f.Name + ">"));

            Dictionary<Type, string> map = new Dictionary<Type, string>();
            map[typeof(string)] = "";
            map[typeof(int)] = "atoi";
            map[typeof(bool)] = "PARSEBOOL";
            map[typeof(char)] = "";
            map[typeof(short)] = "(short)atoi";
            map[typeof(float)] = "(float)atof";
            map[typeof(double)] = "atof";
            map[typeof(byte)] = "(char)atoi";
            map[typeof(long)] = "atol"; // FIXME: Check this

            if (map.ContainsKey(f.FieldType))
                cf.WriteLine(
@"  SKIPBLANKS();
  MATCH({0});
  if (!(p = find(buf, {1}, nc))) return 0;
  *(p - 1) = '\0';
  dest->{2} = {3}(buf);
  buf = p;
  if ((nc = len - (int)(buf - start)) < 0) return 0;
", openmsg, closemsg, f.Name, map[f.FieldType]);
            else if (f.FieldType.IsEnum)
                cf.WriteLine(
@"  SKIPBLANKS();
  MATCH({0});
  if (!(p = find(buf, {1}, nc))) return 0;
  *(p - 1) = '\0';
  dest->{2} = {3}_read(buf);
  buf = p;
  if ((nc = len - (int)(buf - start)) < 0) return 0;
", openmsg, closemsg, f.Name, f.FieldType.Name);
            else // FIXME: do not handle nested structs of the same type!
                cf.WriteLine(
@"  SKIPBLANKS();
  MATCH({0});
  if (!(p = find(buf, {1}, nc))) return 0;
  *(p - 1) = '\0';
  {3}_Read(&dest->{2}, buf, p - buf);
  buf = p;
  if ((nc = len - (int)(buf - start)) < 0) return 0;
", openmsg, closemsg, f.Name, map[f.FieldType]);
        }

        static void GenerateRead(Type t, String bc, StreamWriter cf)
        {
            cf.WriteLine();
            cf.WriteLine("int {0}_Read({0}* dest, char* buf, int len)", t.Name);
            cf.WriteLine("{");
            cf.WriteLine(
@"  char *start = buf;
  char *p;
  int nc = len;
  
  SKIPBLANKS();
  MATCH({0})
  SKIPBLANKS();
  MATCH({1});
",  xbmg.OpenXmlTagCode, bc);

            VisitFields(t, new FieldProcessor(GenerateReadField), t, cf);

            cf.WriteLine("  return 1;");
            cf.WriteLine("}");
        }

        static void GenerateWriteField(Type t, FieldInfo f, StreamWriter cf)
        {
            string openmsg = string.Format("{0}", xbmg.GenerateNextBinaryCode("<" + f.Name + ">"));
            string closemsg = string.Format("{0}", xbmg.GenerateNextBinaryCode("</" + f.Name + ">"));
            cf.WriteLine("  *dest = {0};", openmsg);
            cf.WriteLine("  CHK(1);");

            Dictionary<Type, string> map = new Dictionary<Type,string>();
            map[typeof(string)] = "%s";
            map[typeof(int)] = "%d";
            map[typeof(char)] = "%c";
            map[typeof(short)] = "%d";
            map[typeof(float)] = "%f";
            map[typeof(double)] = "%f";
            map[typeof(byte)] = "%c";
            map[typeof(long)] = "%d"; // FIXME: Check this
            if (map.ContainsKey(f.FieldType))
                cf.WriteLine("  CHK(sprintf(dest, \"{0}\", src->{1}));", map[f.FieldType], f.Name);
            else if (f.FieldType.IsEnum)
                cf.WriteLine("  CHK({0}_write(src->{1}, dest, len));", f.FieldType.Name, f.Name);
            else if (f.FieldType == typeof(bool))
                cf.WriteLine("  CHK(sprintf(dest, src->{0} ? \"true\" : \"false\"));", f.Name);
            else
                cf.WriteLine("  CHK({0}_write(&src->{1}, dest, len));", f.FieldType.Name, f.Name);

            cf.WriteLine("  *dest = {0};", closemsg);
            cf.WriteLine("  CHK(1);");
        }

        static void GenerateWrite(Type t, String bc, String ec, StreamWriter cf)
        {
            string xmlhead = xbmg.OpenXmlTagCode;
            string openmsg = string.Format("{0}", bc);
            string closemsg = string.Format("{0}", ec);

            cf.WriteLine();
            cf.WriteLine("char* {0}_Write({0}* src, char* dest, int len)", t.Name);
            cf.WriteLine("{");
            cf.WriteLine("  *dest = {0};", "0x02"); //start of text code
            cf.WriteLine("  CHK(1);");
            cf.WriteLine("  *dest = {0};", xmlhead);
            cf.WriteLine("  CHK(1);");
            cf.WriteLine("  *dest = {0};", openmsg);
            cf.WriteLine("  CHK(1);");

            VisitFields(t, new FieldProcessor(GenerateWriteField), t, cf);

            cf.WriteLine("  *dest = {0};", closemsg);
            cf.WriteLine("  CHK(1);");
            cf.WriteLine("  *dest = {0};", "0x03"); //end of text code
            cf.WriteLine("  CHK(1);");

            cf.WriteLine("  return dest;");
            cf.WriteLine("}");
        }

        static void GenerateEnumRead(Type t, StreamWriter cf)
        {
            cf.WriteLine();
            cf.WriteLine("int {0}_read(char* s) {{", t.Name);
            foreach (FieldInfo f in t.GetFields())
            {
                if (f.FieldType != t) continue;
                if ((int)f.GetValue(null) == -1)
                    Console.WriteLine("Dhe, ma popo -1 as enum dovevi sceglie... vedi che {0}_read va rivista a mano o mi patchi...", t.Name);
                cf.WriteLine("  if (!strcmppgm2ram(s, \"{0}\")) return {1};", f.Name, (int)f.GetValue(null));
            }
            cf.WriteLine("  return -1;");
            cf.WriteLine("}");
        }

        static void GenerateEnumWrite(Type t, StreamWriter cf)
        {
            cf.WriteLine();
            cf.WriteLine("int {0}_write(int v, char* dest, int len) {{", t.Name);
            cf.WriteLine("  switch (v) {");
            foreach (FieldInfo f in t.GetFields())
            {
                if (f.FieldType != t) continue;
                cf.WriteLine("    case {0}: CHK(sprintf(dest, \"{1}\")); return {2};", (int)f.GetValue(null), f.Name, f.Name.Length);
            }
            cf.WriteLine("  }");
            cf.WriteLine("  return 0;");
            cf.WriteLine("}");
        }

        static void GenerateFunctions(Type t, StreamWriter cf)
        {
            if (t.IsEnum)
            {
                GenerateEnumRead(t, cf);
                GenerateEnumWrite(t, cf);
            }
            else
            {
                String bc = xbmg.GenerateNextBinaryCode("<" + t.Name + " xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns:xsd=\"http://www.w3.org/2001/XMLSchema\">");
                String ec = xbmg.GenerateNextBinaryCode("</" + t.Name + ">");
                GenerateStartsWith(t, bc, cf);
                GenerateRead(t, bc, cf);
                GenerateWrite(t, bc, ec, cf);
            }
        }

        static void GenerateTypeInterface(Type t)
        {
            Console.WriteLine("Generating interface for type {0}", t);
            StreamWriter hf = File.CreateText(string.Format("Messages\\{0}.h", t.Name));

            hf.WriteLine("#ifndef _{0}_H_", t.Name.ToUpper());
            hf.WriteLine("#define _{0}_H_", t.Name.ToUpper());
            hf.WriteLine();

            if (t.IsEnum)
                GenerateEnum(t, hf);
            else
                GenerateStruct(t, hf);

            hf.WriteLine();
            hf.WriteLine(string.Format("#endif // _{0}_H_", t.Name.ToUpper()));
            hf.WriteLine();

            hf.Close();

            StreamWriter cf = File.CreateText(string.Format("Messages\\{0}.c", t.Name));

            cf.WriteLine("#include \"{0}.h\"", t.Name);
            cf.WriteLine("#include <string.h>");
            cf.WriteLine("#include <stdlib.h>");
            cf.WriteLine("#include <stdio.h>");
            cf.WriteLine();
            cf.WriteLine("#define CHK(l) { int i = (l); dest += i; len -= i; if (len < 0) return 0; }");
            cf.WriteLine("#define MATCH(TOK)   if (!(buf = match(buf, TOK))) return 0;\\");
            cf.WriteLine("                     if ((nc = len - (int)(buf - start)) < 0) return 0;");
            cf.WriteLine("#define GOTO(TOK)	 buf = find(buf, TOK, nc); \\");
            cf.WriteLine("                   if ((nc = len - (int)(buf - start)) < 0) return 0;");
            cf.WriteLine("#define SKIPBLANKS()   buf = skipBlanks(buf, nc); \\");
            cf.WriteLine("                       if ((nc = len - (int)(buf - start)) < 0) return 0;");
            cf.WriteLine("#define PARSEBOOL(buf) (strcmppgm2ram(buf, \"true\") == 0)");
            cf.WriteLine();
            cf.WriteLine(
@"static char* skipBlanks(char* s, int len) {
  while (len-- && *s && (*s == 0x20 || *s == 0x09 || *s == 0x0a || *s == 0x0d)) s++;
  return s;
}

static char* match(char* b, char t) {
  if (*b == t)
    return b + 1;
  return 0;
}

static char* find(char* b, char t, int len) {
  while (len-- && *b && !match(b, t)) b++;
  if (len && *b)
    return b + 1;
  return 0;
}
");
            GenerateFunctions(t, cf);

            cf.Close();
        }

        static void GenerateNetOS(Dictionary<Type, bool> TypesToGenerate)
        {
            StreamWriter f = File.CreateText("uTNetOS\\uTNetOS.h");
            f.WriteLine("#ifndef _UTNETOS_H_");
            f.WriteLine("#define _UTNETOS_H_");
            f.WriteLine();
            f.WriteLine("#include \"heap.h\"");
            f.WriteLine("#include \"shared_timer.h\"");
            f.WriteLine("#include \"listeners.h\"");
            f.WriteLine("#include \"uTNetOSMessages.h\"");
            f.WriteLine();
            f.WriteLine("#include \"Pic18.h\"");
            f.WriteLine();
            f.WriteLine(@"
typedef near int (*STARTS_WITH)(char* buf, int len);
typedef near int (*READMSG)(void* dest, char* buf, int len);
typedef near char* (*WRITEMSG)(void* src, char* dest, int len);

// FIXME: Perhaps it is better to have it generated from RobletMessage
typedef struct _uTNetOS_RobletMessage {
  char* Name;
  int RobletID;
  char* BrokerHost;
  int BrokerPort;
  PacketType Type;
} uTNetOS_RobletMessage;

void uTNetOS_Init(const rom char * robletname, long brokerip, short brokerport, long bodymapip, short bodymapport);
void uTNetOS_DoCycle();
void uTNetOS_ListenForEvent(FSABase* w, uTNetOS_RobletMessages msg);
void uTNetOS_RemoveForEvent(FSABase* w, uTNetOS_RobletMessages msg);
void uTNetOS_SendMessage(WRITEMSG fw, void* src);
short uTNetOS_BodyMapPort();
short uTNetOS_BrokerPort();
char* uTNetOS_RobletName();

long uTNetOS_BodyMapIP();
long uTNetOS_BrokerIP();

extern Timer timer;");
            f.WriteLine("#define SECS(s) (s * 900) //900 gained by simulations");
            f.WriteLine("#define MILLIS(s) (s)");
            f.WriteLine();
            f.WriteLine("#endif // _UTNETOS_H_");
            f.Close();

            f = File.CreateText("uTNetOS\\uTNetOS.c");
            f.WriteLine("#include \"uTNetOS.h\"");
            f.WriteLine();
            f.WriteLine("#include <p18f452.h>");
            f.WriteLine("#include <usart.h>");
            f.WriteLine("#include <stdlib.h>");
            f.WriteLine("#include <timers.h>");
            f.WriteLine("#include \"sralloc.h\"");
            f.WriteLine();
            f.WriteLine("#include \"uTNetOSMessagesInternal.h\"");
            f.WriteLine("#if PICDEBUG");
            f.WriteLine("char timestr[10];");
            f.WriteLine("#endif");
            f.WriteLine();

            f.WriteLine("/*** Begin buffers section ***/");
            f.WriteLine("#pragma udata inbuff");
            f.WriteLine("char inBuf[MAX_BUF];");
            f.WriteLine("#pragma udata");
            f.WriteLine("");
            f.WriteLine("#define IncomingBuf inBuf");
            f.WriteLine("");
            f.WriteLine("#pragma udata incomingBufVariables");
            f.WriteLine("volatile char msgInIncomingBuf = 0;");
            f.WriteLine("volatile int IncomingBuffIndex = 0;");
            f.WriteLine("volatile int IncomingBufLen = 0;");
            f.WriteLine("");
            f.WriteLine("#pragma udata readbuff");
            f.WriteLine("char readBuf[MAX_BUF];");
            f.WriteLine("#pragma udata");
            f.WriteLine("");
            f.WriteLine("#pragma udata outbuff");
            f.WriteLine("char outBuf[MAX_BUF];");
            f.WriteLine("#pragma udata");
            f.WriteLine("");
            f.WriteLine("#define ReadBuf readBuf");
            f.WriteLine("#define OutcomingBuf outBuf");
            f.WriteLine("");
            f.WriteLine("#define MSGBUF outBuf");
            f.WriteLine("#define  MSGBUFSZ MAX_BUF");
            f.WriteLine(@"
/*** End buffers section ***/

/*** Begin Time section ***/
Timer timer;
int _clock_count = 0;
/*** End Time section ***/

/*** Begin ISR section ***/
void high_isr(void);");
            f.WriteLine();
            f.WriteLine("#pragma code high_vector=0x08");
            f.WriteLine(@"
void interrupt_at_high_vector(void)
{
_asm GOTO high_isr _endasm
}");
            f.WriteLine("#pragma code");
            f.WriteLine();
            f.WriteLine("#pragma interrupt high_isr");
            f.WriteLine(@"
void high_isr (void)
{
    /*** Usart receiver interrupt handler ***/
    if ((PIR1bits.RCIF) && (PIE1bits.RCIE)) //usart interrupt enabled and flag bit
    {
       unsigned char c;
       if (RCSTAbits.OERR) //overrun error, do nothing for now
       {
         RCSTAbits.CREN = 0;
         RCSTAbits.CREN = 1;
       }
       if (RCSTAbits.FERR) //frame error, do nothing for now
           c = RCREG; //clear FERR simply reading RCREG 

	   //read char from usart
       c = RCREG;

	   if (c == 0x02) //start of text control character
       {
         IncomingBuffIndex = 0;
         return;
       }
       if (c == 0x03) //end of text control character
	   { 
	     IncomingBuf[IncomingBuffIndex] = '\0'; //end string
         putcUSART(XOFF); //only one message can be buffered
	     msgInIncomingBuf = 1;
         IncomingBufLen = IncomingBuffIndex;
         IncomingBuffIndex = 0;
	   }
	   else
	   {
	     IncomingBuf[IncomingBuffIndex] = c;
	     IncomingBuffIndex++;
         if (IncomingBuffIndex == MAX_BUF)
         {
           /* 
            This only for avoid buffer overflow error. The transmitter must be
            send only messages of max length MAX_BUF
           */
           IncomingBuffIndex = 0;   
         }
	   }
       return;
    }

    /*** Timer0 interrupt handler ***/
    if ((INTCONbits.TMR0IF) && (INTCONbits.TMR0IE)) //timer interrupt enabled and flag bit
    {
		if (_clock_count + 1 < 0)
	    {
		  TimerSetZeroTime(&timer);
	      _clock_count = -1;
	    }
	    _clock_count++;
        INTCONbits.TMR0IF = 0; // Clear the interrupt flag
        return;
    }
}
/*** End ISR section ***/

long _broker_ip; //on pic int are on 16 bit
char _broker_ip_s[32];
short _broker_port;
long _bodymap_ip;
short _bodymap_port;
char * _roblet_name;

long uTNetOS_BodyMapIP() {
	return _bodymap_ip;
}

short uTNetOS_BodyMapPort() {
	return _bodymap_port;
}

long uTNetOS_BrokerIP() {
	return _broker_ip;
}

short uTNetOS_BrokerPort() {
	return _broker_port;
}

char* uTNetOS_RobletName() {
	return _roblet_name;
}

void uTNetOS_ListenForEvent(FSABase* w, uTNetOS_RobletMessages msg) {
  listeners_Insert(&listeners[msg], w);
}

void uTNetOS_RemoveForEvent(FSABase* w, uTNetOS_RobletMessages msg) {
  listeners_Remove(&listeners[msg], w);
}

void _SendMessage() 
{
#if PICDEBUG
    itoa(_clock_count,timestr);
    putrsUSART(""Sending message at Time(ms):"");
    putsUSART(timestr);
    putrsUSART(""\r\n"");
#endif
    putsUSART(MSGBUF);
#if PICDEBUG
    putrsUSART(""\r\n"");
#endif
}

int usart_eventlistener(char evt) 
{
    int i;
#if PICDEBUG
    itoa(_clock_count,timestr);
    putrsUSART(""Receiving at Time(ms):"");
    putsUSART(timestr);
    putrsUSART("" this message:\r\n"");
    putsUSART(IncomingBuf);
    putrsUSART(""\r\n"");
#endif
    switch(evt) {
		case USART_EVENT_DATA:
            if (MetaDataRequest_startsWith(IncomingBuf, IncomingBufLen)) 
            {
				char *p;
				MetaDataRequest req;
				MetaDataRequest_Read(&req, IncomingBuf, IncomingBufLen);
			    RobletInterface_Write(_roblet_name, req.BrokerHost, req.BrokerPort, MSGBUF, MSGBUFSZ);
				_SendMessage();
            } else {
			    for (i = 0; i < known_messages_length; i++) {
				    if (known_messages[i](IncomingBuf, IncomingBufLen)) {
					    known_messages_read[i](ReadBuf, IncomingBuf, IncomingBufLen);
				   	    listeners_Notify(&listeners[i], ReadBuf, i);
			    	}
			    }
            }
			break;
		default:
    		putrsUSART(""Unknown message event""); //FIX ME
	}
	return( 0 ); 
}

void uTNetOS_Init(const rom char * robletname, long brokerip, short brokerport, long bodymapip, short bodymapport) 
{
     int i;

	//define a dynamic heap for Pic's SRAM. Necessary for malloc, calloc, free, memset, strdup
    SRAMInitHeap();

	_broker_ip = brokerip;
	_broker_port = brokerport;
	_bodymap_ip = bodymapip;
	_bodymap_port = bodymapport;
    _roblet_name = (char *) strdup(robletname);

	sprintf(_broker_ip_s, ""%d.%d.%d.%d"", IP1(_broker_ip), IP2(_broker_ip), IP3(_broker_ip), IP4(_broker_ip));

	// FIXME: perhaps in future version the initial size should be exposed
	heap_Init(&(timer.priorityQueue), 8);

	for (i = 0; i < 1; i++)
	  listeners_Init(&listeners[i], 1);

    /* Configure timer0 for 4 Mhz oscillator
       Prescaler of 4 --> timer0 overflow after 1 ms 
    */
    OpenTimer0(T0_8BIT & T0_SOURCE_INT & T0_PS_1_4);
    WriteTimer0(0);

    /* Initialize system clock */
    _clock_count = 0;


    /* Open the USART configured as
       8N1, 9600 baud, in polled mode
    */
    OpenUSART (USART_TX_INT_OFF &
               USART_RX_INT_ON &
               USART_ASYNCH_MODE &
               USART_EIGHT_BIT &
               USART_CONT_RX &
               USART_BRGH_HIGH, 25);

    /* Configure high priority interrupts */
    RCONbits.IPEN = 1;
    /* Set high priority for Usart receiver interrupt */
    IPR1bits.RCIP = 1;
    /* Enable all high priority interrupts */
    INTCONbits.GIEH = 1;

    putcUSART(XON); //tell to transmitter we are ready


#if PICDEBUG
    itoa(_clock_count,timestr);
    putrsUSART(""Simulation beginning at Time(ms):"");
    putsUSART(timestr);
    putrsUSART(""\r\n\r\n"");
#endif
}

void usart_tick()
{
	if (msgInIncomingBuf)
    {
      usart_eventlistener(USART_EVENT_DATA);
      msgInIncomingBuf = 0;
      putcUSART(XON);
    }
}

void uTNetOS_DoCycle() 
{
    usart_tick();
    TimerDoStep(&timer);

    #if PICRELEASE
        //reset Watch Dog Timer	
        _asm clrwdt _endasm
    #endif
}

void uTNetOS_SendMessage(WRITEMSG fw, void* source) 
{
	uTNetOS_RobletMessage *msg = (uTNetOS_RobletMessage*)source;

	msg->BrokerHost = _broker_ip_s;
	msg->BrokerPort = _broker_port;
    msg->Name = _roblet_name;
	msg->RobletID = 0;
	msg->Type = PacketType_Data;

	fw(source, MSGBUF, MSGBUFSZ);
	_SendMessage();
}
");
            f.Close();
            bool first = true;
            StringBuilder sb = new StringBuilder();
            StringBuilder mn = new StringBuilder();
            f = File.CreateText("uTNetOS\\uTNetOSMessages.h");
            foreach (Type t in TypesToGenerate.Keys)
            {
                if (!t.IsSubclassOf(typeof(Robotics4.NET.RobletMessage))) continue;
                if (first) first = false;
                else mn.Append(", ");
                
                sb.AppendLine(string.Format("#include \"..\\Messages\\{0}.h\"", t.Name));
                mn.AppendFormat("RobletMessages_{0}", t.Name);
            }
            f.WriteLine("#ifndef _UTNETOSMESSAGES_H__");
            f.WriteLine("#define _UTNETOSMESSAGES_H__");
            f.WriteLine();
            f.WriteLine("#include \"uTNetOSRobletInterface.h\"");
            f.WriteLine("{0}",sb.ToString());
            f.WriteLine();
            f.WriteLine("typedef enum {{ {0} }} uTNetOS_RobletMessages;",mn.ToString());
            f.WriteLine();
            f.WriteLine("#endif // _UTNETOSMESSAGES_H__");
            f.Close();

            StringBuilder km = new StringBuilder();
            StringBuilder kmr = new StringBuilder();
            StringBuilder kmw = new StringBuilder();
            first = true;
            int count = 0;
            foreach (Type t in TypesToGenerate.Keys)
            {
                if (!t.IsSubclassOf(typeof(Robotics4.NET.RobletMessage))) continue;
                count++;
                if (first) first = false;
                else
                {
                    km.Append(", ");
                    kmr.Append(", ");
                    kmw.Append(", ");
                }
                km.Append(string.Format("{0}_startsWith", t.Name));
                kmr.Append(string.Format("(READMSG){0}_Read", t.Name));
                kmw.Append(string.Format("(WRITEMSG){0}_Write", t.Name));
            }
            f = File.CreateText("uTNetOS\\uTNetOSMessagesInternal.h");
            f.WriteLine("#ifndef _UTNETOSMESSAGESINTERNAL_H_");
            f.WriteLine("#define _UTNETOSMESSAGESINTERNAL_H_");
            f.WriteLine(@"
STARTS_WITH known_messages[] = {{ {0} }};
READMSG known_messages_read[] = {{ {1} }};
WRITEMSG known_messages_write[] = {{ {2} }};
int known_messages_length = {3};
Listeners listeners[{3}]; // Tipo dei messaggi", km.ToString(), kmr.ToString(), kmw.ToString(), count);
            f.WriteLine();
            f.WriteLine("#endif // _UTNETOSMESSAGESINTERNAL_H_");
            f.Close();
        }

        static void Main(string[] args)
        {
            if (!Directory.Exists("Messages"))
                Directory.CreateDirectory("Messages");
            if (!Directory.Exists("uTNetOS"))
                Directory.CreateDirectory("uTNetOS");
            TypesToGenerate = new Dictionary<Type, bool>();

            xbmg = new XmlBinManager.MapGenerator();
            
            foreach (string s in args)
            {
                if (!File.Exists(s))
                {
                    Console.WriteLine("Could not find {0}, skipping...", s);
                    continue;
                }
                string fn = (new FileInfo(s)).FullName;
                Assembly a = Assembly.LoadFile(fn);
                foreach (Type t in a.GetTypes())
                {
                    if (t.IsSubclassOf(typeof(Robotics4.NET.RobletMessage)))
                    {
                        TypesToGenerate[t] = false;
                        //GenerateTypeInterface(t);
                        //RobletTest.Beat rt = new RobletTest.Beat();
                        //XmlSerializer ser = new XmlSerializer(t);
                        //ser.Serialize(Console.Out, rt);
                    }
                    else if (t.IsSubclassOf(typeof(Robotics4.NET.RobletBase)))
                    {
                        Robotics4.NET.RobletBase rob = (Robotics4.NET.RobletBase)Activator.CreateInstance(t);
                        Robotics4.NET.RobletInterface interf;
                        Type[] types;
                        Robotics4.NET.RobletBroker.Init();
                        rob.GetRobletInterface(out interf, out types);
                        interf.RobletID = 0;
                        interf.BrokerPort = 0;
                        interf.BrokerHost = "placeholder";
                        interf.Name = "placeholder";
                        XmlSerializer ser = new XmlSerializer(interf.GetType(), null, types, null, null, (new FileInfo(fn)).Directory.FullName, null);
                        MemoryStream mem = new MemoryStream();
                        ser.Serialize(mem, interf);
                        string data = System.Text.UTF8Encoding.UTF8.GetString(mem.GetBuffer());
                        data = System.Text.RegularExpressions.Regex.Replace(data, "<BrokerHost>[^<]*</BrokerHost>", "<BrokerHost><<HOST>></BrokerHost>");
                        data = System.Text.RegularExpressions.Regex.Replace(data, "<BrokerPort>[^<]*</BrokerPort>", "<BrokerPort><<PORT>></BrokerPort>");
                        data = System.Text.RegularExpressions.Regex.Replace(data, "<RobletID>[^<]*</RobletID>", "<RobletID><<ID>></RobletID>");
                        data = System.Text.RegularExpressions.Regex.Replace(data, "<Name>[^<]*</Name>", "<Name><<NAME>></Name>");
                        data = System.Text.RegularExpressions.Regex.Replace(data, "</RobletInterface>.*", "</RobletInterface>");
                        //data = data.Replace("\\", "\\\\");
                        //data = data.Replace("\r", "");
                        //data = data.Replace("\n", "\\r\\n");
                        //data = data.Replace("\"", "\\\"");
                        //string[] frags = System.Text.RegularExpressions.Regex.Split(data, "(<(?:BrokerHost|BrokerPort|RobletID|Name)>[^<]*</(?:BrokerHost|BrokerPort|RobletID|Name)>)");

                        StreamWriter f = File.CreateText("uTNetOS\\uTNetOSRobletInterface.h");
                        f.WriteLine("#ifndef _UTNETOSROBLETINTERFACE_H__");
                        f.WriteLine("#define _UTNETOSROBLETINTERFACE_H__");
                        f.WriteLine();
                        f.WriteLine("char* RobletInterface_Write(char* robletname, char* host, int port, char* dest, int len);");
                        f.WriteLine();
                        f.WriteLine("#endif // _UTNETOSROBLETINTERFACE_H__");
                        f.Close();

                        f = File.CreateText("RobletInterface.xml");
                        f.WriteLine(data);
                        f.Close();

                        f = File.CreateText("uTNetOS\\uTNetOSRobletInterface.c");
                        f.WriteLine("#include \"uTNetOSRobletInterface.h\"");
                        f.WriteLine("#include <string.h>");
                        f.WriteLine("#include <stdlib.h>");
                        f.WriteLine("#include <stdio.h>");
                        f.WriteLine();
                        f.WriteLine("#define CHK(l) { int i = (l); dest += i; len -= i; if (len < 0) return 0; }");
                        f.WriteLine();
                        f.WriteLine("char* RobletInterface_Write(char* robletname, char* host, int port, char* dest, int len) {");
                        f.WriteLine("  *dest = {0};", "0x02"); //start of text code
                        f.WriteLine("  CHK(1);");
                        f.WriteLine("  *dest = {0};", xbmg.OpenXmlTagCode);
                        f.WriteLine("  CHK(1);");
                        String binCode = xbmg.GenerateNextBinaryCode("<RobletInterface InterfaceOnBodyMap=\"");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        f.WriteLine("  CHK(sprintf(dest, \"%s\", robletname));");
                        binCode = xbmg.GenerateNextBinaryCode(".xml\" xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns:xsd=\"http://www.w3.org/2001/XMLSchema\">");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        binCode = xbmg.GenerateNextBinaryCode("<Name>");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        f.WriteLine("  CHK(sprintf(dest, \"%s\", robletname));");
                        binCode = xbmg.GenerateNextBinaryCode("</Name>");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        binCode = xbmg.GenerateNextBinaryCode("<RobletID>");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        f.WriteLine("  CHK(sprintf(dest, \"{0}\"));", interf.RobletID);
                        binCode = xbmg.GenerateNextBinaryCode("</RobletID>");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        binCode = xbmg.GenerateNextBinaryCode("<BrokerHost>");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        f.WriteLine("  CHK(sprintf(dest, \"%s\", host));");
                        binCode = xbmg.GenerateNextBinaryCode("</BrokerHost>");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        binCode = xbmg.GenerateNextBinaryCode("<BrokerPort>");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        f.WriteLine("  CHK(sprintf(dest, \"%d\", port));");
                        binCode = xbmg.GenerateNextBinaryCode("</BrokerPort>");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        binCode = xbmg.GenerateNextBinaryCode("<Type>");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        f.WriteLine("  CHK(sprintf(dest, \"Data\"));");
                        binCode = xbmg.GenerateNextBinaryCode("</Type>");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        binCode = xbmg.GenerateNextBinaryCode("</RobletInterface>");
                        f.WriteLine("  *dest = {0};", binCode);
                        f.WriteLine("  CHK(1);");
                        f.WriteLine("  *dest = {0};", "0x03"); //end of text code
                        f.WriteLine("  CHK(1);");
                        f.WriteLine("  return dest;");
                        f.WriteLine("}");
                        f.Close();
                    }
                }
            }
            TypesToGenerate[typeof(Robotics4.NET.MetaDataRequest)] = false;

            bool exit = false;
            while (!exit)
            {
                exit = true;
                foreach (Type t in TypesToGenerate.Keys)
                {
                    if (!TypesToGenerate[t])
                    {
                        GenerateTypeInterface(t);
                        TypesToGenerate[t] = true;
                        exit = false;
                        break;
                    }
                }
            }
            GenerateNetOS(TypesToGenerate);
            
            XmlWriterSettings settings = new XmlWriterSettings();
            settings.Indent = true;
            settings.NewLineOnAttributes = true;
            XmlWriter writer = XmlWriter.Create("XmlBinMap.xml", settings);
            xbmg.ConversionMap.WriteXml(writer);
            writer.Flush();
            writer.Close();
        }
    }
}
#endif
